#include "PoseFrame.h"

#include "G2oTypes.h"
#include "KeyPoseFrame.h"
#include "Converter.h"

#include <thread>

using namespace std;

namespace msf
{

// 下一个生成的帧的ID,这里是初始化类的静态成员变量
long unsigned int PoseFrame::nNextId=0;

// 是否要进行初始化操作的标志
// 这里给这个标志置位的操作是在最初系统开始加载到内存的时候进行的，下一帧就是整个系统的第一帧，所以这个标志要置位
bool PoseFrame::mbInitialComputations=true;

PoseFrame::PoseFrame(): mpcpi(NULL), mpImuPreintegrated(NULL), mpPrevPoseFrame(NULL), mpImuPreintegratedPoseFrame(NULL), mpReferenceKPF(static_cast<KeyPoseFrame*>(NULL)), mbIsSet(false), mbImuPreintegrated(false), mbHasPose(false), mbHasVelocity(false)
{}

// 拷贝构造函数
PoseFrame::PoseFrame(const PoseFrame &frame)
    :mpcpi(frame.mpcpi), mTimeStamp(frame.mTimeStamp), mImuCalib(frame.mImuCalib), mpImuPreintegrated(frame.mpImuPreintegrated), mpImuPreintegratedPoseFrame(frame.mpImuPreintegratedPoseFrame), 
     mImuBias(frame.mImuBias), mnId(frame.mnId), mpReferenceKPF(frame.mpReferenceKPF), mpPrevPoseFrame(frame.mpPrevPoseFrame), mpLastKeyPoseFrame(frame.mpLastKeyPoseFrame),
     mbIsSet(frame.mbIsSet), mbImuPreintegrated(frame.mbImuPreintegrated), mpMutexImu(frame.mpMutexImu), mTow(frame.mTow), mbHasPose(false), mbHasVelocity(false)
{
      if(frame.mbHasPose)
            SetPose(frame.GetPose());

      if(frame.HasVelocity())
      {
            SetVelocity(frame.GetVelocity());
      }

}

// IMU+ODOM模式
PoseFrame::PoseFrame(const nav_msgs::OdometryConstPtr &odom, const double &timestamp, PoseFrame* pPrevPF, const IMU::Calib &ImuCalib)
    :mpcpi(NULL), mTimeStamp(timestamp), mImuCalib(ImuCalib), mpImuPreintegrated(NULL), mpImuPreintegratedPoseFrame(NULL), 
     mpReferenceKPF(static_cast<KeyPoseFrame*>(NULL)), mpPrevPoseFrame(pPrevPF),
     mbIsSet(false), mbImuPreintegrated(false), mbHasPose(false), mbHasVelocity(false)
{
    // Frame ID
    // 帧的ID自增
    mnId=nNextId++;

    // Problem1
    //odom_data = std::make_shared<OdomData>();

    // nav_msgs::Odometry &odom信息处理
    odom_data.position[0] = odom->pose.pose.position.x;
    odom_data.position[1] = odom->pose.pose.position.y;
    odom_data.position[2] = odom->pose.pose.position.z;

    // 构造四元数
    Eigen::Quaterniond q(
        odom->pose.pose.orientation.w,
        odom->pose.pose.orientation.x,
        odom->pose.pose.orientation.y,
        odom->pose.pose.orientation.z);

    // 通过四元数构造旋转矩阵
    Eigen::Matrix3d rot_mat = q.toRotationMatrix();    

    // 通过旋转矩阵获取欧拉角
    Eigen::Vector3d euler_angles = rot_mat.eulerAngles(0, 1, 2);

    // 将欧拉角赋值给odom_ptr的attitude成员
    odom_data.attitude[0] = euler_angles[0];
    odom_data.attitude[1] = euler_angles[1];
    odom_data.attitude[2] = euler_angles[2];

    odom_data.linear_vel[0] = odom->twist.twist.linear.x;
    odom_data.linear_vel[1] = odom->twist.twist.linear.y;
    odom_data.linear_vel[2] = odom->twist.twist.linear.z;

    odom_data.angular_vel[0] = odom->twist.twist.angular.x;
    odom_data.angular_vel[1] = odom->twist.twist.angular.y;
    odom_data.angular_vel[2] = odom->twist.twist.angular.z;

    // V(R) : measurement noise covariance init
    Eigen::VectorXd noise_vec(6);
    noise_vec << 0.0001,0.0001,0.0001,25.0,25.0,25.0; // 0.0001m/s,25rad/s(0.0005,20.0)
    odom_data.cov.setZero();
    odom_data.cov.block<6, 6>(3, 3) = noise_vec.asDiagonal();

    if(pPrevPF)
    {
        if(pPrevPF->HasVelocity())
        {
            SetVelocity(pPrevPF->GetVelocity());
        }
    }
    else
    {
        mVw.setZero();
    }

    mpMutexImu = new std::mutex();
}

bool PoseFrame::isSet() const {
    return mbIsSet;
}

/** 
 * @brief 赋值位姿
 * @param Tow 位姿:从世界坐标系到odom坐标系的变换,也就是odom在世界坐标系下的位姿
 */
void PoseFrame::SetPose(const Sophus::SE3<float> &Tow)
{
    mTow = Tow;

    UpdatePoseMatrices();
    mbIsSet = true;
    mbHasPose = true;
}

/** 
 * @brief 赋值新的偏置
 * @param b 偏置
 */
void PoseFrame::SetNewBias(const IMU::Bias &b)
{
    mImuBias = b;
    if(mpImuPreintegrated)
        mpImuPreintegrated->SetNewBias(b);
}

/** 
 * @brief 赋值新的速度
 * @param Vwb 速度
 */
void PoseFrame::SetVelocity(Eigen::Vector3f Vwb)
{
    mVw = Vwb;
    mbHasVelocity = true;
}

/** 
 * @brief 获取速度
 */
Eigen::Vector3f PoseFrame::GetVelocity() const
{
    return mVw;
}

/** 
 * @brief 赋值位姿与速度
 */
void PoseFrame::SetImuPoseVelocity(const Eigen::Matrix3f &Rwb, const Eigen::Vector3f &twb, const Eigen::Vector3f &Vwb)
{
    mVw = Vwb;
    mbHasVelocity = true;

    Sophus::SE3f Twb(Rwb, twb);
    Sophus::SE3f Tbw = Twb.inverse();
    // imu系转到odom系
    mTow = mImuCalib.mTob * Tbw;

    UpdatePoseMatrices();
    mbIsSet = true;
    mbHasPose = true;
}

/** 
 * @brief 通过mTcw更新其他关于位姿的变量
 */
void PoseFrame::UpdatePoseMatrices()
{
    Sophus::SE3<float> Two = mTow.inverse();
    mRwo = Two.rotationMatrix();
    mOw = Two.translation();
    mRow = mTow.rotationMatrix();
    mtow = mTow.translation();
}

/** 
 * @brief 获得imu的平移
 */
Eigen::Matrix<float,3,1> PoseFrame::GetImuPosition() const {
    return mRwo * mImuCalib.mTob.translation() + mOw;
}

/** 
 * @brief 获得imu的旋转
 */
Eigen::Matrix<float,3,3> PoseFrame::GetImuRotation() {
    return mRwo * mImuCalib.mTob.rotationMatrix();
}

/** 
 * @brief 获得imu的位姿
 */
Sophus::SE3<float> PoseFrame::GetImuPose() {
    return mTow.inverse() * mImuCalib.mTob;
}

/** 
 * @brief 是否做完预积分
 */
bool PoseFrame::imuIsPreintegrated()
{
    unique_lock<std::mutex> lock(*mpMutexImu);
    return mbImuPreintegrated;
}

/** 
 * @brief 设置为做完预积分
 */
void PoseFrame::setIntegrated()
{
    unique_lock<std::mutex> lock(*mpMutexImu);
    mbImuPreintegrated = true;
}

}